#ifndef CONTROL_H__
#define CONTROL_H__

#include "system.h"
#include "mpu6xxx.h"
#include "servoio.h"
#include "fibcom.h"
#include "stdio.h"

//#define STUPID_TRACING

#define MAGNITUDE_THRESH 10 /* magnitude小于此值则认为没有发现直线 */
#define DEFAULT_SPEED (SERVO_MID + SERVO_HALF/16) /* 直线行驶的速度所对应的PWM脉宽 */
#define THETA_THRESH 30

extern float kp_theta, ki_theta, kd_theta, kp_rho, ki_rho, kd_rho;

void controld(void *arg);

void messaged(void *arg);

#endif /* CONTORL_H__ */
